package flca.osm.perst.util;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;

import org.garret.perst.FieldIndex;
import org.garret.perst.Index;
import org.garret.perst.Key;

import flca.osm.perst.model.PNode;
import flca.osm.perst.model.PWay;
import flca.osm.util.MapUtils;
import flca.voicenav.VoiceNavUtilsConstants;
import flca.voicenav.model.LatLng;
import flca.voicenav.model.TrkPt;
import flca.voicenav.model.TrkPtCorner;
import flca.voicenav.util.DistHelper;

public class MapPerstUtil implements MapUtils, VoiceNavUtilsConstants
{
	private static int sRange = 50 * METER_E6;
	
	@Override
	public void setDefaultRange(int aRange)
	{
		sRange  = aRange;
	}


	@Override
	public PNode getNode(int aNodeId)
	{
		return PerstDbHelper.getInstance().getRoot().nodePk.get(aNodeId);
	}

	public FieldIndex<PNode> getNodeLatIndex()
	{
		return PerstDbHelper.getInstance().getRoot().nodeLat;
	}
	
	@Override
	public PWay getWay(int aWayId)
	{
		return PerstDbHelper.getInstance().getRoot().wayPk.get(aWayId);
	}

	@Override
	public List<PWay> getConnectedWays(PNode aNode) 
	{
		List<PWay> r = new ArrayList<PWay>();
		
		if (aNode != null) {
			if (aNode.getWays() != null) {
				for (Integer wayid : aNode.getWays()) {
					PWay way = getWay(wayid);
					if (way != null) {
						if (way.isClosed()) {
							r.addAll(getConnPWaysViaRoundabout(aNode, way));
						} else {
							r.add(getWay(wayid));
						}
					} else {
						System.out.println("way is null for " + wayid);
					}
				}
			}
		}
		
		return r;
	}


	
	public void findNextWay(int aCornerRefNode, List<LatLng> aCoords, NavigateData aNavData)
	{
		PNode currCorner = getNode(aCornerRefNode);
		List<PWay> connWays = getConnectedWays(currCorner);
		
		if (currCorner != null && connWays != null && connWays.size() > 0) {
			//find the coord that corresponds with the corner
			int lastCoordIdx = getCorrCoordIndex(currCorner, aCoords);
			int currCoordIdx = findCoordClosestToCorner(currCorner.getLatlng(), aCoords, lastCoordIdx);
			
			//now find one of the connected way's and its corr next corner refnode that matches the next coord
			WayNodes nextWay = findNextWayViaConnWays(connWays, aCoords, currCorner, currCoordIdx);
			aNavData.add(nextWay);
			if (aNavData.getLastNode() != null) {
				aNavData.lastCoordIndex = findCoordClosestToCorner(aNavData.getLastNode().getLatlng(), aCoords, currCoordIdx);
			}
		}
	}

	@Override
	public List<TrkPt> navigate(List<LatLng> aCoords)
	{
		return doNavigate(aCoords);
	}

	@Override
	public List<TrkPt> testGetNextWay(int aCornerRefNode, List<LatLng> aCoords, int aLastCoordIndex)
	{
		NavigateData navdata = new NavigateData();
		findNextWay(aCornerRefNode, aCoords, navdata);
		return addTrkPts(navdata);
	}

	//----------- private methodes --------------------

	private List<TrkPt>  doNavigate(List<LatLng> aCoords)
	{
		NavigateData navdata = new NavigateData();
		
		boolean goon = true;
		int startRefNode = findStartNodeRef(aCoords);
		
		int prevSize = 0; int count = 0;
		findNextWay(startRefNode, aCoords, navdata);
		while (goon) {
//			System.out.println(result.nextRefNodeId());
			findNextWay(navdata.nextRefNodeId(), aCoords, navdata);
			goon = goonNavigating(navdata, aCoords, prevSize, count++);
			prevSize = navdata.getNodes().size();
		}
		
		// finally add all the TrkPt's 
		return addTrkPts(navdata);
	}

	private boolean goonNavigating(NavigateData aNavData, List<LatLng> aCoords, int aPrevSize, int count) 
	{
		if (aPrevSize == aNavData.getNodes().size()) { //stop if we did not grow
			return false;
		} else if (aNavData.lastCoordIndex == aCoords.size()-1) {
			return false;
		} else if (count > 5 * aCoords.size()) {
			return false;
		} else {
			return true;
		}
	}
	
	

	/*
	 * this returns a WayNodes (that is a (partial) PWay), that is going into the direction of the route
	 */
	private WayNodes findNextWayViaConnWays(List<PWay> aConnWays, List<LatLng> aCoords, PNode aCurrCorner, int aCurrCoordIdx)
	{
		List<WayNodes> connWaynodes = findConnWays(aCurrCorner, aConnWays);

		List<Double> distances = new ArrayList<Double>();
		for (WayNodes waynodes : connWaynodes) {
			distances.add(calcDistToWay(waynodes, aCoords, aCurrCoordIdx));
		}
		
		return connWaynodes.get(getBestConnWayIdx(distances));
	}

	private int getBestConnWayIdx(List<Double> distances )
	{
		int bestAt = -1; int i = 0;
		double minDist = 0;
		for (Double dist : distances) {
			if (bestAt < 0 || dist < minDist) {
				minDist = dist;
				bestAt = i;
			}
			i++ ;
		}
		return bestAt;
	}

	private static final double FIND_BESTWAY_DIST = 100 * METER * E6;
	/*
	 * here we calc what's the 'distance' from the route to the given. We do this by going the next waynode that is 
	 * far enough from the current corner (to be realistic). Then we calc the distance to the coord that is approx equally 
	 * far away from the current corner. 
	 * Note we do it so, because it is possible the same corner is passed on the route twice (or more) via different directions 
	 * (see the 8 in boogies extreme)  
	 */
	private double calcDistToWay(WayNodes aWayData, List<LatLng> aCoords, int aCurrCoordIdx)
	{
		//first find a waynode far enough from curr node, or the next corner.
		Map<PNode, Double> calcNode = findNodeToCalcDist(aWayData);
		//now find coord more-or-less equally far away, 
		LatLng calcCoord = findCoordToCalcDist(calcNode.values().iterator().next(), aCoords, aCurrCoordIdx);
		
		return DistHelper.calcDistance(calcNode.keySet().iterator().next().getLatlng(), calcCoord);
	}

	private List<PWay>  getConnPWaysViaRoundabout(PNode startRoundaboutCorner, PWay aPWay) 
	{
		// first get all other corners
		List<PNode> roundAboutCorners = findRoundaboutCorners(aPWay, startRoundaboutCorner.getId());
		
		// and find all PWays that are connected to one of the roundabout corners
		return findRoundaboutConnPways(roundAboutCorners, aPWay.getId(), startRoundaboutCorner);
	}

	private List<PNode> findRoundaboutCorners(PWay aPWay, int aSkipCornerRefnode)
	{
		List<PNode> roundAboutCorners = new ArrayList<PNode>();
		for (int refNodeId : aPWay.getWayNodes()) {
			PNode node = getNode(refNodeId);
			if (aSkipCornerRefnode != node.getId() && node.isCorner() ) {
				roundAboutCorners.add(node);
			}
		}
		return roundAboutCorners;
	}

	private List<PWay> findRoundaboutConnPways(List<PNode> roundAboutCorners, int skipRoundaboutWayId, PNode startRoundaboutCorner)
	{
		List<PWay> result = new ArrayList<PWay>();
		
		for (PNode node : roundAboutCorners) {
			if (node.isCorner()) {
				for (int wayid : node.getWays()) {
					if (wayid != skipRoundaboutWayId) {
						PWay way = makeRoundaboutConnWay(wayid, node, startRoundaboutCorner);
						if (way != null) {
							result.add(way);
						}
					}
				}
			}
		}
		
		return result;
	}

	/*
	 * returns the connected PWay, but also modifies the wayNodes to include the corner refnode where we entered the roundabout
	 */
	private PWay makeRoundaboutConnWay(int aWayId, PNode thisCorner, PNode startRoundaboutCorner) 
	{
		PWay way = getWay(aWayId);
		
		if (way != null && way.getWayNodes() != null && way.getWayNodes().size() > 0) {
			int at = way.getWayNodes().indexOf(thisCorner.getId());
			way.getWayNodes().add(at, startRoundaboutCorner.getId());
		}
		return way;
	}
	
	private Map<PNode, Double> findNodeToCalcDist(WayNodes aWayData)
	{
		int useIdx = -1;
		double totalDist = 0.0;
		for (int i=1; i < aWayData.waynodes.size(); i++) {
			double dist = DistHelper.calcDistance(aWayData.getNode(i-1).getLatlng(), 
												  aWayData.getNode(i).getLatlng());
			totalDist += dist;
			if (totalDist > FIND_BESTWAY_DIST) {
				useIdx = i;
				break;
			}
		}
		PNode useNode = (useIdx > 0) ? aWayData.getNode(useIdx) : aWayData.nextCornerRefnode();
		HashMap<PNode, Double> result = new HashMap<PNode, Double>();
		result.put(useNode, totalDist);
		return result;
	}
	
	private LatLng findCoordToCalcDist(double aUseDist, List<LatLng> aCoords, int aCurrCoordIdx)
	{
		int useIdx = -1;
		double totalDist = 0.0;
		for (int i=aCurrCoordIdx+1; i < aCoords.size(); i++) {
			double dist = DistHelper.calcDistance(aCoords.get(i-1), aCoords.get(i));
			totalDist += dist;
			if (totalDist + 10 > aUseDist) {
				useIdx = i;
				break;
			}
		}
		LatLng usePoint = (useIdx > 0) ? aCoords.get(useIdx) : aCoords.get(aCoords.size()-1);
		return usePoint;
	}

	private List<WayNodes> findConnWays(PNode aCorner, List<PWay> aConnWays)
	{
		List<WayNodes> result = new ArrayList<MapPerstUtil.WayNodes>();

		for (PWay pWay : aConnWays) {
			WayNodes waynodes[] = getWayNodesFromCurrRefnode(pWay, aCorner);
			for (WayNodes wn : waynodes) {
				result.add(wn);
			}
		}
		
		return result;
	}
	
	/*
	 * 
	 */
	private WayNodes[] getWayNodesFromCurrRefnode(PWay aWay, PNode aRefNode)
	{
		int at = findNodeIdx(aWay, aRefNode);
		
		if (at==0 ) {
			return new WayNodes[] {makeWayData(aWay, aRefNode, at, true)};
		} else if (at==aWay.getWayNodes().size()-1) {
			return new WayNodes[] {makeWayData(aWay, aRefNode, at, false)};
		} else {
			return new WayNodes[] {makeWayData(aWay, aRefNode, at, true),
								  makeWayData(aWay, aRefNode, at, false)};
		}
	}
	
	/* 
	 * this returns the index if the given coords list
	 */
	private int getCorrCoordIndex(LatLng aCoord, List<LatLng> aCoords)
	{
		double minDist = -1;
		int foundAt = -1;
		for (int i = 0; i < aCoords.size(); i++) {
			double dist = DistHelper.calcDistance(aCoord, aCoords.get(i));
			if (minDist < 0 || dist < minDist) {
				minDist = dist;
				foundAt = i;
			}
		}
		return foundAt;
	}

	private int getCorrCoordIndex(PNode aPnode, List<LatLng> aCoords) {
		return getCorrCoordIndex(aPnode.getLatlng(), aCoords);
	}
	

	private int findStartNodeRef(List<LatLng> aCoords)
	{
		int result = -1;
		
		LatLng startCoord = aCoords.get(0);

		Map<Integer, Double> distances = new HashMap<Integer, Double>();
		for (PNode node : getNodeLatIndex().iterator(
				new Key(startCoord.getLat() - sRange, true), // inclusive low boundary
				new Key(startCoord.getLat() + sRange, true), // exclusive high boundary
				Index.ASCENT_ORDER)) {
			
			if (node.isCorner()) {
				distances.put(node.getId(), DistHelper.calcDistance(startCoord, node.getLatlng()));
			}
		}

		double minDist = -1;
		
		for (Integer nodeid : distances.keySet()) {
			double d = distances.get(nodeid);
			if (minDist < 0 || d < minDist) {
				minDist = d;
				result = nodeid;
			}
			
		}

		return result;
	}


	private List<TrkPt> addTrkPts(NavigateData aNavdata)
	{
		List<TrkPt> trkpts = new ArrayList<TrkPt>();
		for (PNode node : aNavdata.getNodes()) {
			TrkPt tp = makeTrkPt(node);
			trkpts.add(tp);
		}
		return trkpts;
	}
	
	private TrkPt makeTrkPt(PNode node)
	{
//		System.out.println(refnode + " " + node);
		TrkPt trkpt = new TrkPt();
//		trkpt.setWayNode(node.getId());
		trkpt.setLat(node.getLat()/1E6);
		trkpt.setLng(node.getLng()/1E6);
		if (node.isCorner()) {
			trkpt.setCorner(new TrkPtCorner()); //TODO fill up!
		}
		return trkpt;
	}

	private WayNodes makeWayData(PWay aWay, PNode aRefNode, int at, boolean aUpWards)
	{
		List<PNode> waynodes = findNextCorner(aWay, aRefNode, aUpWards);
		return new WayNodes(aWay.getId(), waynodes);
	}
	
	/* 
	 * crawl along the coords until the best match is found. first the distance will become smaller, and than 
	 * larger again the prev coord is the one we are looking for.
	 */
	private int findCoordClosestToCorner(LatLng aNode, List<LatLng> aCoords, int aLastCoordIndex)
	{
		int r = aLastCoordIndex;
		
		double minDist = -1.0;
		for (r = aLastCoordIndex; r < aCoords.size(); r++) {
			double dist = DistHelper.calcDistance(aNode, aCoords.get(r));
			if (minDist < 0 ) {//first round
				minDist = dist;
			} else {
				if (dist < minDist) {
					minDist = dist;
				} else {
					break;
				}
			}
		}
		
		return r - 1;
	}

	/*
	 * returns the index of a RefNode in a Way
	 */
	private int findNodeIdx(PWay aWay, PNode aNode)
	{
		return findNodeIdx(aWay, aNode.getId());
	}

	/*
	 * returns the index of a RefNode in a Way
	 */
	private int findNodeIdx(PWay aWay, int aRefNodeId)
	{
		for (int i=0; i < aWay.getWayNodes().size(); i++) {
			if (aWay.getWayNodes().get(i) == aRefNodeId) {
				return i;
			}
		}
		throw new RuntimeException("no refnode found " + aRefNodeId + " on way " + aWay);
	}

	private List<PNode> findNextCorner(PWay aWay, PNode aFromNode, boolean aUpWards)
	{
		List<PNode> result = new ArrayList<PNode>();
		
		int startAt = findNodeIdx(aWay, aFromNode);
		
		if (aUpWards) {
			for (int i=startAt; i < aWay.getWayNodes().size(); i++) {
				boolean isCorner = addWayNode(aWay, result, i);
				if (i > startAt && isCorner) break;
			}
		} else {
			for (int i=startAt; i >= 0; i--) {
				boolean isCorner = addWayNode(aWay, result, i);
				if (i < startAt && isCorner) break;
			}
		}
		return result; 
	}


	private boolean addWayNode(PWay aWay, List<PNode> result, int i)
	{
		PNode node = getNode(aWay.getWayNodes().get(i));
		result.add(node);
		return node.isCorner();
	}


	//-- return class --
	class NavigateData 
	{
		List<WayNodes> wayNodes = new ArrayList<MapPerstUtil.WayNodes>();
		List<CoordIdxRefNode> coordRefnodesLinks = new ArrayList<MapPerstUtil.CoordIdxRefNode>();
		int lastRefNodeAdded = 0;
		int lastCoordIndex = -1;
		
		int wayNodesSize() {
			return wayNodes.size();
		}
		
		void add(WayNodes aValue) {
			wayNodes.add(aValue);
		}
		

		CoordIdxRefNode getCoordIdxRefnode(int aRowNr) {
			if (coordRefnodesLinks.size() > 0 && aRowNr >= 0 && aRowNr < coordRefnodesLinks.size()) {
				return coordRefnodesLinks.get(aRowNr);
			} else {
				return null;
			}
		}

		CoordIdxRefNode getLastCoordIdxRefnode() {
			return getCoordIdxRefnode(coordRefnodesLinks.size()-1);
		}

		int nextRefNodeId() {
			if (getNodes().size()>0) {
				return getNodes().get(getNodes().size()-1).getId();
			} else {
				return -1;
			}
		}
		
		/*
		 * shortcut to get list of PNode's , and this one also filters out matching end-begin waynodes
		 */
		List<PNode> getNodes()
		{
			List<PNode> result = new ArrayList<PNode>();
			
			for (WayNodes wn : wayNodes) {
				for (PNode node : wn.waynodes) {
					if (result.size() == 0 || result.get(result.size()-1).getId() != node.getId()) {
						result.add(node);
					}
				}
			}
			return result;
		}
		
		PNode getLastNode() 
		{
			if (getNodes().size()>0) {
				return getNodes().get(getNodes().size()-1);
			} else {
				return null;
			}
		}

		@Override
		public String toString()
		{
			return "NavigateData [lastRefNodeAdded=" + lastRefNodeAdded + ", wayNodes=" + wayNodes + "]";
		}
	}
	
	//--------------------------------------------------
	
	private class WayNodes 
	{
		int wayId;
		List<PNode> waynodes = new ArrayList<PNode>();

		WayNodes(int aWayId, List<PNode> aWaynodes) {
			wayId = aWayId;
			waynodes = aWaynodes;
		}

		PNode getNode(int idx) {
			if (waynodes.size() > 0 && idx >= 0 && idx < waynodes.size()) {
				return waynodes.get(idx);
			} else {
				return null;
			}
		}

		PNode nextCornerRefnode() {
			if (waynodes.size() > 0) {
				return waynodes.get(waynodes.size()-1);
			} else {
				return null;
			}
		}
		
		@Override
		public String toString()
		{
			String s = " wayid=" + wayId + ", nodes=";
			for (PNode node : waynodes) {
				s += node.getId() + ",";
			}
			return s + "]";
		}
	}
	
	//-------------------------------------------------------
	class CoordIdxRefNode {
		int refnodeId;
		int coordIdx;
	
		public CoordIdxRefNode(int refnodeId, int coordIdx)
		{
			super();
			this.refnodeId = refnodeId;
			this.coordIdx = coordIdx;
		}

		@Override
		public int hashCode()
		{
			final int prime = 31;
			int result = 1;
			result = prime * result + getOuterType().hashCode();
			result = prime * result + coordIdx;
			result = prime * result + refnodeId;
			return result;
		}

		@Override
		public boolean equals(Object obj)
		{
			if (this == obj)
				return true;
			if (obj == null)
				return false;
			if (getClass() != obj.getClass())
				return false;
			CoordIdxRefNode other = (CoordIdxRefNode) obj;
			if (!getOuterType().equals(other.getOuterType()))
				return false;
			if (coordIdx != other.coordIdx)
				return false;
			if (refnodeId != other.refnodeId)
				return false;
			return true;
		}

		private MapPerstUtil getOuterType()
		{
			return MapPerstUtil.this;
		}
		
		
	}
}
